#include <string>
#include <csignal>
#include <glog/logging.h>
#include <yaml-cpp/yaml.h>
#include "basic/LoopFunc.h"
#include "ros/ros1node.h"

std::string curl_ipv4 = "\" http://192.168.1.102:8666/sys/receiveMapFile";
std::shared_ptr<ROS1Node> node;
int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    FLAGS_log_dir = "/home/yhs/log";
    google::ParseCommandLineFlags(&argc, &argv, true);

    ros::init(argc, argv, "ros_midware");
    auto nh = std::make_shared<ros::NodeHandle>("~");

    std::string config_yaml = "/home/yhs/.wrks/hkprinter/HKPrinter/config/config.yaml";
    if (argc == 2) {
        config_yaml = argv[1];
    }
    nh->param<std::string>("config_yaml", config_yaml, config_yaml);

    LOG(INFO) << "[MIDWARE] Load yaml from " << config_yaml;
    auto yaml = YAML::LoadFile(config_yaml);
    std::string udp_server_ipv4 = "";
    std::string udp_client_ipv4 = "";
    uint16_t udp_server_port = 0;
    std::string map_path = "/home/yhs/maps";
    std::string curl_ip = "192.168.0.102";
    try {
        udp_server_ipv4 = yaml["UDP_SERVER_IPV4"].as<std::string>();
        udp_client_ipv4 = yaml["UDP_CLIENT_IPV4"].as<std::string>();
        udp_server_port = yaml["UDP_SERVER_PORT"].as<uint16_t>();
        map_path = yaml["MAPS_PATH"].as<std::string>();
        curl_ip = yaml["CURL_IPV4"].as<std::string>();
    } catch (...) {
        LOG(ERROR) << "[MIDWARE] Failed to parse yaml";
        std::exit(EXIT_FAILURE);
    }
    curl_ipv4 = "\" http://" + curl_ip + ":8666/sys/receiveMapFile";
    node = std::make_shared<ROS1Node>(nh, udp_server_ipv4, udp_server_port);
    node->set_map_path(map_path);
    node->thread_UDPRecv();

    LoopFunc loop_op("loop_op", 0.1, 3, std::bind(&ROS1Node::thread_op, node));
    LoopFunc loop_send("loop_send", 1, 3, std::bind(&ROS1Node::thread_UDPSend, node));
    
    loop_op.detach();
    loop_send.detach();

    ros::AsyncSpinner spinner(0);
    spinner.start();
    ros::waitForShutdown();
    // Final
    ros::shutdown();
    node->stop();
    return EXIT_SUCCESS;
}